/* roboboca.c -- arquivo principal da biblioteca roboboca
 *
 * Copyright (C) 2008, Diogo F. S. Ramos <diogofsr@grad.icmc.usp.br>
 *                     Bruno Ribeiro de Oliveira
 *                     Arineiza Cristina Pinheiro
 *                     Tomas Grover <tomas.grover@usp.br>
 * 
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

/** @file
 * Arquivo principal da biblioteca roboboca.
 */

/* Defines */
/** Velocidade máxima que um robô pode andar pra frente */
#define MAX_SPEED 1
/** Velocidade mínima que um robô pode andar pra frente */
#define MIN_SPEED 0.5
/** Cor do blob que os robôs devem procurar  */
#define BLOB_COLOR 0x00FF0000	/* RGB - vermelho */

/*#define BLOB_COLOR 0x0000FF00*/ /* RGB - verde*/



#include <stdio.h>
#include <config.h>
#include <math.h>
#include <pthread.h>
#include <unistd.h>

#include "xerror.h"
#include "xmalloc.h"
#include "roboboca.h"

/* roboboca_t *****************************************************************/

/** Cria uma nova estrutura roboboca.

 Este comando cria uma nova estrutura roboboca para acomodar os outros
 robôs e a estrutura global de blobs.

 @param n_robots Número de robôs que a estrutura deve comportar.

 @return Uma nova estrutura roboboca.
*/
struct roboboca_t *
roboboca_new( int n_robots )
{
    struct roboboca_t *box = NULL;
    int i = 0;

    box = XMALLOC( struct roboboca_t, 1 );

    box->n_robots = n_robots;
    box->l_robots = XMALLOC( roboboca_robot_t*, box->n_robots );
    for( i = 0 ; i < box->n_robots ; i++ )
    {
	box->l_robots[i] = NULL;
    }
    for( i = 0; i < 5; i++)
    {
	box->vec_blob_pos[i].ta_la = BLOB_VAZIO;
	box->vec_blob_pos[i].x = 0;
	box->vec_blob_pos[i].y = 0;
    }

    /* Iniciando o Mutex */
    box->mutex = XMALLOC( pthread_mutex_t, 1 );
    pthread_mutex_init( box->mutex,
			NULL );

    box->all_blobs_finded = 0;
    box->all_blobs_returned = 0;
    
    return box;
}

/** Limpa uma estrutura roboboca.

    Limpa uma estrutura roboboca para que não haja vazamento de memória.
    
    @param box Estrutura roboboca a ser limpa e destruída.
    @see roboboca_new() 
    @return Sucesso.
*/
int
roboboca_destroy( struct roboboca_t *box )
{
    int i = 0;

    for( i = 0 ; i < box->n_robots ; i++ )
    {
	if( box->l_robots[i] != NULL )
	{
	    roboboca_robot_destroy( box->l_robots[i] );
	}
    }

    pthread_mutex_destroy( box->mutex );
    XFREE( box->mutex );
    
    XFREE( box );

    return EXIT_SUCCESS;
}

/** Adiciona roboboca_robot_t a estrutura

    Adiciona, no fim a estrutura, robos criados atraves do comando roboboca_robot_new()

    @param box Estrutura roboboca.
    @param robot Robô a ser adicionado.
    @see roboboca_robot_new() 
    @return Sucesso.
*/
int
roboboca_append_robot( roboboca_t * box,
		       roboboca_robot_t *robot )
{
    int i = 0;
    int filled = 0;

    for( i = 0 ; i < box->n_robots ; i++ )
    {
	if( box->l_robots[i] == NULL )
	{
	    box->l_robots[i] = robot;
	    filled = 1;
	    break;
	}
    }

    if( filled == 0 )
    {
	xerror_error ("Estrutura com o numero maximo de robos ocupados");
	return EXIT_FAILURE;
    }

    return EXIT_SUCCESS;
}
/******************************************************************************/

/* roboboca_robot_t ***********************************************************/

/** Cria um novo robô.

    Cria um novo robô que possui laser, gripper e outros
    mecanismo. Dentra da estrutura há também um client para
    conectar-se no servidor.

    Como a biblioteca player necessita de um cliente para registar os
    mecanismos como laser e etc e o cliente precisa de um endereço de
    servidor, para criamos o robô é preciso adicionar também o
    endereço e porta do servidor. Também é necessário passar as
    posições do robô no sistema global.

    @param x_orig Posição X global.
    @param y_orig Posição Y global.
    @param server Endereço do servidor.
    @param port Porta do servidor a se conectar.
    @return Ponteiro para o novo robô criado.
*/
roboboca_robot_t *
roboboca_robot_new( const double x_orig,
		    const double y_orig,
		    const char *server,
		    const int port )
{
    roboboca_robot_t * robot = NULL;

    robot = XMALLOC( roboboca_robot_t, 1 );

    robot->client = NULL;
    robot->laser = NULL;
    robot->position2d = NULL;
    robot->blobfinder = NULL;
    robot->gripper = NULL;

    /* Setando as posicoes iniciais */
    robot->x_orig = x_orig;
    robot->y_orig = y_orig;

    robot->client = playerc_client_create( NULL,
					   server,
					   port );
    if( playerc_client_connect(robot->client) )
    {
	xerror_error("Não consegui conectar no servidor");
	playerc_client_destroy( robot->client );
	XFREE( robot );
	return NULL;
    }
    
    robot->laser = playerc_laser_create( robot->client,
					 0 );
    if( playerc_laser_subscribe(robot->laser, PLAYERC_OPEN_MODE) )
    {
	xerror_error("Não consegui registrar o laser no servidor");
	playerc_laser_destroy( robot->laser );
	playerc_client_disconnect( robot->client );
	playerc_client_destroy( robot->client );
	XFREE( robot );
	return NULL;
    }
    
    robot->position2d = playerc_position2d_create( robot->client,
						   0 );
    if( playerc_position2d_subscribe(robot->position2d, PLAYERC_OPEN_MODE) )
    {
	xerror_error("Não consegui registrar o odometro no servidor");
	playerc_position2d_destroy( robot->position2d );
	playerc_laser_unsubscribe( robot->laser );
	playerc_laser_destroy( robot->laser );
	playerc_client_disconnect( robot->client );
	playerc_client_destroy( robot->client );
	XFREE( robot );
	return NULL;
    }

    robot->blobfinder = playerc_blobfinder_create( robot->client,
						   0 );
    if( playerc_blobfinder_subscribe(robot->blobfinder, PLAYERC_OPEN_MODE) )
    {
	xerror_error("Não consegui registrar o blobfinder no servidor");
	playerc_blobfinder_destroy( robot->blobfinder );
	playerc_position2d_unsubscribe( robot->position2d );
	playerc_position2d_destroy( robot->position2d );
	playerc_laser_unsubscribe( robot->laser );
	playerc_laser_destroy( robot->laser );
	playerc_client_disconnect( robot->client );
	playerc_client_destroy( robot->client );
	XFREE( robot );
	return NULL;
    }

    robot->gripper = playerc_gripper_create( robot->client,
					     0 );
    if( playerc_gripper_subscribe(robot->gripper, PLAYERC_OPEN_MODE) )
    {
	xerror_error("Não consegui registrar o blobfinder no servidor");
	playerc_gripper_destroy( robot->gripper );
	playerc_blobfinder_unsubscribe( robot->blobfinder );
	playerc_blobfinder_destroy( robot->blobfinder );
	playerc_position2d_unsubscribe( robot->position2d );
	playerc_position2d_destroy( robot->position2d );
	playerc_laser_unsubscribe( robot->laser );
	playerc_laser_destroy( robot->laser );
	playerc_client_disconnect( robot->client );
	playerc_client_destroy( robot->client );
	XFREE( robot );
	return NULL;
    }

    return robot;
}

/** Limpa uma estrutura roboboca_robot_t

    Limpa uma estrutura roboboca_robot_t para que não haja vazamento de memória.

    @param robot Estrutura roboboca_robot_t a ser limpa e destruída.
    @see roboboca_robot_new() 
    @return Sucesso.
*/
int
roboboca_robot_destroy( roboboca_robot_t * robot )
{
    if( robot->gripper != NULL )
    {
	playerc_gripper_unsubscribe( robot->gripper );
	playerc_gripper_destroy( robot->gripper );
    }

    if( robot->blobfinder != NULL )
    {
	playerc_blobfinder_unsubscribe( robot->blobfinder );
	playerc_blobfinder_destroy( robot->blobfinder );
    }
    
    if( robot->laser != NULL )
    {
	playerc_laser_unsubscribe( robot->laser );
	playerc_laser_destroy( robot->laser );
    }

    if( robot->position2d != NULL )
    {
	playerc_position2d_unsubscribe( robot->position2d );
	playerc_position2d_destroy( robot->position2d );
    }

    if( robot->client != NULL )
    {
	playerc_client_disconnect( robot->client );
	playerc_client_destroy( robot->client );
    }
    
    return EXIT_SUCCESS;
}

/******************************************************************************/

/* Aqui começa realmente a estratégia *****************************************/

/** Manda um robô para a posição X,Y.

    O robô vai até a posição enviada e a função só retorna depois que o robô chega.

    @param *box Coleção de robôs
    @param *robot Robô a ser comandado.
    @param px Posição X global.
    @param py Posição Y global.
    @return Sucesso quando o robô chega na posição enviada.
*/
int
roboboca_goto_simple( roboboca_t * box,
		      roboboca_robot_t *robot,
		      double px,
		      double py,
		      int searching)
{
    int numero_total_lasers = 18;
    int i = 0 ;
    double xdelta = 0;
    double ydelta = 0;
    double xdelta_aux = 0;
    double ydelta_aux = 0;
    double vangularfinal = 0;
    double vfinal = 0;
    double x_blob = 0;
    double y_blob = 0;

    do
    {
	playerc_client_read( robot->client );
	xdelta = 0;
	ydelta = 0;
		
	
	/* Encontrando as caixinhas =) */
	/* Ideias (c/ blobfinder):
	   -descobrir a distancia do robo p/ caixinha
	   1) girar o robo até ficar de fronte com a caixinha
	   2) calcular o angulo entre a caixinha e o eixo-x
	*/

	/* Marcando blobs */
	/* Aqui vai o que podemos chamar de HACK. Se o robô virar
	 * muito rápido, o blobfinder fica bobo. Então, caso a
	 * velocidade de rotação seja maior que a marcada ai embaixo,
	 * o robô não procura por blobs. */
	if( searching == 1)
	{
	    if( (box->all_blobs_finded == 0) &&
		( fabs(robot->position2d->va) < 0.4 ) &&
		(searching == 1) )
	    {
		for( i = 0 ; i < robot->blobfinder->blobs_count ; i++ )
		{
		    if( robot->blobfinder->blobs[i].color == BLOB_COLOR )
		    {
			roboboca_get_blob_position( robot->blobfinder->blobs[i].x,
						    robot->blobfinder->blobs[i].range,
						    robot->position2d->px+robot->x_orig,
						    robot->position2d->py+robot->y_orig,
						    robot->position2d->pa,
						    &x_blob,
						    &y_blob );
			roboboca_mark_blob( box,
					    x_blob,
					    y_blob );
		    }
		}
	    }else if( (box->all_blobs_finded != 0) )
	    {
		return EXIT_SUCCESS;
	    }
	}
	
	/* Procurando obstáculos */
	for (i = 0 ; i < 361 ; i += (360/numero_total_lasers)+1 )
	{
	    roboboca_AvoidObstacle( robot->laser->scan[i][0],
	    			    roboboca_laser_to_global_rad( robot->position2d->pa,
	    							  robot->laser->scan[i][1]),
	    			    &xdelta_aux,
	    			    &ydelta_aux);
	    xdelta += xdelta_aux;
	    ydelta += ydelta_aux;
	}

	/* Correndo em direção ao objetivo */	
	roboboca_SeekGoal( robot,
		     px,
		     py,
	 	     &xdelta_aux,
		     &ydelta_aux );

	xdelta += xdelta_aux;
	ydelta += ydelta_aux;
	
	/* Cálculo do ângulo */
	vangularfinal = atan2( (ydelta),(xdelta) );
	/* Ângulo final */
	vangularfinal = roboboca_minor_distance_rad( vangularfinal, robot->position2d->pa );
	/* Velocidade final */	
	vfinal = (sqrt( pow( xdelta, 2) +
			pow( ydelta, 2) ));

	/* Arrumando a velocidade para que ele não pare nem vá muito
	 * rápido */
	if (vfinal > MAX_SPEED)
	{
	    vfinal = MAX_SPEED;
	} else if( vfinal < MIN_SPEED )
	    vfinal = MIN_SPEED;

	/* Caso o campo de repulsão esteja jogando o robô muito para
	 * uma direção contrária. Assim da tempo do robô virar sem
	 * bater. */
	if( fabs(vangularfinal) > M_PI_2 )
	    vfinal = MIN_SPEED;

	/* Multiplica o valor de rotação para que ele seja
	 * significativo */
	vangularfinal *= 3;

	/* Não deixa que a rotação seja muito alta */
	if( vangularfinal > 2 )
	    vangularfinal = 2;
	else if( vangularfinal < -2 )
	    vangularfinal = -2;
	
	playerc_position2d_set_cmd_vel( robot->position2d,
					vfinal,
					0,
					vangularfinal,
					1);
    }while( (fabs(roboboca_robot_get_global_px(robot)-px) > 0.4) ||
	    (fabs(roboboca_robot_get_global_py(robot)-py) > 0.4));

    /* Parando o robô */
    playerc_position2d_set_cmd_vel(robot->position2d,
				   0,
				   0,
				   0,
				   1);    
    return EXIT_SUCCESS;
}

/** Transforma ângulo do laser em ângulo global.
    
    O ângulo do laser é dado em radianos e tem como referência o
    robô. Esta função transforma-o em um ângulo com base no sistema
    global.

    @param pa Ângulo do robô no sistema global.
    @param laser Ângulo do laser no sistema do robô.

    @return Ângulo do laser no sistema global.
 */
double
roboboca_laser_to_global_rad( const double pa, const double laser )
{
    double result = 0;

    result = pa + laser;

    if( result > M_PI )
    {
	result = -M_PI+(pa-M_PI_2);
    }else if (result < -M_PI)
    {
	result = M_PI+(pa+M_PI_2);
    }

    return result;
}

/** Calcula o vetor V da força de repulsão

    Realiza mesma função que roboboca_AvoidObstacle() porém utilizando
    outra fórmula de cálculo.
    
    @see roboboca_AvoidObstacle()
    @param distance Distância do obstáculo
    @param angle Ângulo do obstáculo em relação ao robô
    @param *xdelta Endereço da variável X do vetor resutaldo
    @param *ydelta Endereço da variável Y do vetor resultado
    @return Sucesso.
*/
int
roboboca_AvoidObstacle2( const double distance,
			 double angle,
			 double *xdelta,
			 double *ydelta)
{
    double res = 0;
    double radius = 0;
    double fallof = 0.7;	/* Raio de influência */
    double alfa = 3;		/* Força do campo */

    if (distance >= radius + fallof)
    {
	res = 0;
    } else if ( (distance > radius) && (distance < radius + fallof) )
    {
	*xdelta = -alfa * (1 / ( pow(distance , 2) - pow(radius, 2) ) ) *
	    ( pow(M_E, (-1 / ( -1*pow( distance,2 ) + pow( radius + fallof, 2 ) ) ) ) ) * cos(angle);
	*ydelta = -alfa * (1 / ( pow(distance , 2) - pow(radius, 2) ) ) *
	    ( pow(M_E, (-1 / ( -1*pow( distance,2 ) + pow( radius + fallof, 2 ) ) ) ) ) * sin(angle);
    }
    return res;
}

/** Calcula o vetor V da força de repulsão

    Utilizando o conceito de campo potencial calcula a força de
    repulsão. Em outras palavras, passe a posição global X e Y que a
    função escreve no xdelta e ydelta o vetor repulsão.

    Diferente da função roboboca_SeekGoal() esta função recebe
    diretamente as informações de distância de angulo pois são
    informações diretamente retiradas do laser.

    @see roboboca_SeekGoal()
    @param distance Distância do obstáculo
    @param angle Ângulo do obstáculo em relação ao robô
    @param *xdelta Endereço da variável X do vetor resutaldo
    @param *ydelta Endereço da variável Y do vetor resultado
    @return Sucesso.
*/
int
roboboca_AvoidObstacle(	const double distance,
			const double angle,
			double * xdelta,
			double * ydelta )
{
    double radius = 0;	        /* Raio do destino */
    double sing_radius = 0.7;	/* Raio de influência */
    double alfa = 4;		/* Força do campo */

    /* Decisão de forças */
    if( distance < radius )	/* Encostou */
    {
	*xdelta = 0;
	*ydelta = 0;
    }
    else if( (radius <= distance) && (distance <= radius + sing_radius ) ) /* Dentro do campo */
    {
	*xdelta = -alfa * ( sing_radius + radius - distance ) * cos(angle);
	*ydelta = -alfa * ( sing_radius + radius - distance ) * sin(angle);
    }
    else			/* Sem força de atração */
    {
	*xdelta = 0;
	*ydelta = 0;
    }

    return EXIT_SUCCESS;
}


/** Calcula o vetor V da força.

    Utilizando o conceito de campo potencial calcula a força de
    atração. Em outras palavras, passe a posição global X e Y que a
    função escreve no xdelta e ydelta o vetor atração.

    @param robot Robô a ser comandado.
    @param xgoal Posição X global do objeto de atração.
    @param ygoal Posição Y global do objeto de atração.
    @param *xdelta Endereço da variável X do vetor resutaldo
    @param *ydelta Endereço da variável Y do vetor resultado
    @return Sucesso.
*/
int
roboboca_SeekGoal( roboboca_robot_t *robot ,
		   double xgoal,
		   double ygoal,
		   double * xdelta,
		   double * ydelta )
{
    double distance = 0;	/* Distância entre a origem e o destino */
    double angle = 0;		/* Angulo da origem e destino */
    double xorig = 0;		/* Origem X */
    double yorig = 0;		/* Origem Y */
    double radius = 0;	        /* Raio do destino */
    double sing_radius = 2;	/* Circulo de singularidade */
    double alfa = 1;		/* Variável para diminuir/aumentar intensidade da força */

    xorig = roboboca_robot_get_global_px(robot);
    yorig = roboboca_robot_get_global_py(robot);
	
    /* Distância entre os dois pontos */
    distance = sqrt( pow( xorig - xgoal, 2) +
		     pow( yorig - ygoal, 2) );
    /* Angulo em relação ao objetivo */
    angle = atan2( (ygoal - yorig),(xgoal-xorig) );

    /* Decisão de forças */
    if( distance < radius )	/* Encostou */
    {
	*xdelta = 0;
	*ydelta = 0;
    }
    else if( (radius <= distance) && (distance <= radius + sing_radius ) ) /* Dentro do campo */
    {
	*xdelta = alfa*(distance-radius)*cos(angle);
	*ydelta = alfa*(distance-radius)*sin(angle);
    }
    else			/* Força máxima de atração */
    {
	*xdelta = alfa*sing_radius*cos(angle);
	*ydelta = alfa*sing_radius*sin(angle);
    }

    return EXIT_SUCCESS;
}

/** Resolve o problema do quadrante

    O angulos são dados através da seguinte lógica
    (-180:+180)=(-PI:+PI) e são dados em radianos. O problema acontece
    quando há mudanças de quadrantes e a distância entre um valor
    positivo e negativo são embaralhados.

    @param goal Ângulo, em radianos, que queremos chegar.
    @param pose Ângulo, em radianos, que estamos.
    @return Menor distância entre os dois ângulos.
*/
double
roboboca_minor_distance_rad( const double goal, const double pose )
{
    double result = 0;
	
    /* Problema do quadrante */
    if( fabs(goal - pose) > M_PI )
    {
	result = (M_PI - fabs(goal)) + (M_PI - fabs(pose));

	if( goal > 0 )
	    result *= -1;
    }
    else
    {
	result = goal - pose;
    }

    return result;
}

/** Pega a posição global do blob observado através do blobfinder

    Algumas informações a se considerar:
      - O x_finder vai de 0 a 79, sendo 0 = PI/6 e 79 = -PI/6
      - O cone de alcance do 0 ao 79 é, aproximadamente de 60 graus, ou PI/3

    @param x_finder Pixel da tela do blob finder no qual o centro do
    blob esta

    @param r_finder Distancia, em centímetros, do centro do blob até o
    robô

    @param px Posição global X do robô

    @param py Posição global Y do robô

    @param pa Ângulo global de direção do robô

    @param *x_blob Posição global X do blob calculada

    @param *y_blob Posição global Y do blob calculada

*/
void
roboboca_get_blob_position( const unsigned int x_finder,
			    const float r_finder,
			    const double px,
			    const double py,
			    const double pa,
			    double * x_blob,
			    double * y_blob )
{
    double angle = 0;

    /* Transformando x_finder em ângulo */
    angle = ( -((x_finder*M_PI)/240) + M_PI/6 );

    /* Transformando um angulo local em um global */
    angle = roboboca_laser_to_global_rad( pa, angle );

    /* Divisão por 1000 para levar para metros */
    *x_blob = px + (r_finder*cos(angle))/1000;
    *y_blob = py + (r_finder*sin(angle))/1000;
}

/** Marca o blob encontrado na estrutura global

    Como esta função acessa um área global, há um mutex para permitir
    que somente um robô a acesse.
    
    @param *box Estrutura que armazena todos os robôs

    @param x_blob Posição X global do blob

    @param y_blob Posição Y global do blob

    @see roboboca_get_blob_position()
*/
int
roboboca_mark_blob( roboboca_t * box,
		    const double x_blob,
		    const double y_blob )
{
    int i = 0;
    double tolerancia = 0.9;
    int ret = EXIT_SUCCESS;
    
    pthread_mutex_lock( box->mutex );

    for( i = 0 ; i < 5 ; i++ )
    {
	if( box->vec_blob_pos[i].ta_la == BLOB_VAZIO )
	{
	    box->vec_blob_pos[i].x = x_blob;
	    box->vec_blob_pos[i].y = y_blob;
	    box->vec_blob_pos[i].ta_la = BLOB_ESPERANDO;

	    if( i == 4 )
	    {
		box->all_blobs_finded = 1;
	    }	    
	    break;
	}else if( box->vec_blob_pos[i].ta_la == BLOB_ESPERANDO )
	{
	    if( (fabs(x_blob - box->vec_blob_pos[i].x) < tolerancia) &&
		(fabs(y_blob - box->vec_blob_pos[i].y) < tolerancia ))
	    {
		ret = EXIT_FAILURE;
		break;
	    }
	}
    }
    
    pthread_mutex_unlock( box->mutex );
    return ret;
}


int
roboboca_get_nearest_blob( roboboca_t * box,
			   roboboca_robot_t * robot )
{
    int i = 0;
    int ret = -1;
    double distance = 0;
    double distance_aux = 0;

    pthread_mutex_lock( box->mutex );
    
    /* Achando primeiro */
    for( i = 0 ; i < BLOB_COUNT ; i++ )
    {
	if( box->vec_blob_pos[i].ta_la == BLOB_ESPERANDO )
	{
	    distance = sqrt( pow( box->vec_blob_pos[i].x - (robot->x_orig + robot->position2d->px ), 2) +
			     pow( box->vec_blob_pos[i].y - (robot->y_orig + robot->position2d->py ), 2));
	    ret = i;
	    break;
	}
    }
    
    for(  ; i < BLOB_COUNT ; i++ )
    {
	if( box->vec_blob_pos[i].ta_la == BLOB_ESPERANDO )
	{
	    distance_aux = sqrt( pow( box->vec_blob_pos[i].x - (robot->x_orig + robot->position2d->px ), 2) +
				 pow( box->vec_blob_pos[i].y - (robot->y_orig + robot->position2d->py), 2));

	    if( distance > distance_aux )
	    {
		distance = distance_aux;
		ret = i;
	    }
	}
    }

    if( ret > -1 )
	/* Marcando blob para ser buscado */
	box->vec_blob_pos[ret].ta_la = BLOB_BUSCANDO;
	
    pthread_mutex_unlock( box->mutex );
    
    return ret;
}

/** Retorna a posição X global do robô
    
    @param *robot Robô

    @return Posição global X
*/
double
roboboca_robot_get_global_px( roboboca_robot_t *robot )
{
    return robot->x_orig + robot->position2d->px;
}

/** Retorna a posição Y global do robô
    
    @param *robot Robô

    @return Posição global Y
*/
double
roboboca_robot_get_global_py( roboboca_robot_t *robot )
{
    return robot->y_orig + robot->position2d->py;
}

int
roboboca_robot_grab_blob( roboboca_t * box,
			  roboboca_robot_t * robot,
			  int blob_i )
{

    roboboca_goto_simple( box,
			  robot,
			  box->vec_blob_pos[blob_i].x,
			  box->vec_blob_pos[blob_i].y,
			  0 );
    roboboca_robot_point_to_blob( robot,
				  box->vec_blob_pos[blob_i].x,
				  box->vec_blob_pos[blob_i].y );
    if (roboboca_robot_focus_on_nearest_blob( robot ) == EXIT_FAILURE )
    {
	roboboca_mark_false_blob( box, blob_i );
	return EXIT_FAILURE;
    }
    else
    {
	roboboca_robot_approach_blob( robot );

	playerc_gripper_set_cmd( robot->gripper, GRIPclose, 0 );
	sleep(2);
    }
			  
    return EXIT_SUCCESS;
}

void
roboboca_mark_false_blob( roboboca_t * box,
			  const int blob_i )
{
    pthread_mutex_lock( box->mutex );
    
    /* Marcando blob para ser buscado */
    box->vec_blob_pos[blob_i].ta_la = BLOB_VAZIO;

    box->all_blobs_finded = 0;
	
    pthread_mutex_unlock( box->mutex );
}

void
roboboca_mark_blob_taken( roboboca_t * box,
			  const int blob_i )
{
    int i = 0;
    int todos_pegos = 1;
    
    pthread_mutex_lock( box->mutex );
    
    /* Marcando blob para ser buscado */
    box->vec_blob_pos[blob_i].ta_la = BLOB_PEGO;

    for( i = 0 ; i < BLOB_COUNT ; i ++ )
    {
	if( box->vec_blob_pos[i].ta_la != BLOB_PEGO )
	    todos_pegos = 0;
    }

    if( todos_pegos )
	box->all_blobs_returned = 1;
	
    pthread_mutex_unlock( box->mutex );
}


void
roboboca_robot_return_blob( roboboca_t * box,
			    roboboca_robot_t * robot,
			    const int blob_i )
{
    roboboca_goto_simple( NULL, robot, robot->x_orig, robot->y_orig, 0 );
    playerc_gripper_set_cmd( robot->gripper, GRIPopen, 0 );
    roboboca_mark_blob_taken( box, blob_i );
}


int
roboboca_robot_approach_blob( roboboca_robot_t * robot )
{
    int i = 0;    
    unsigned int blob_area = -1;
    int blob_i = -1;
    
    do
    {
	playerc_client_read( robot->client );
	blob_area = 0;
	blob_i = 0;

	/* Choose biggest blob */
	for( i = 0 ; i < robot->blobfinder->blobs_count ; i++ )
	{
	    if( robot->blobfinder->blobs[i].color == BLOB_COLOR )
	    {
		if( robot->blobfinder->blobs[i].area > blob_area )
		{
		    blob_area = robot->blobfinder->blobs[i].area;
		    blob_i = i;
		}
	    }
	}

	if( robot->blobfinder->blobs[blob_i].range > 0.1 )
	    playerc_position2d_set_cmd_vel( robot->position2d,
					    0.1,
					    0,
					    0,
					    1);
	else
	    playerc_position2d_set_cmd_vel( robot->position2d,
					    -0.1,
					    0,
					    0,
					    1);
    }while( (robot->blobfinder->blobs[blob_i].range/1000 > 0.22) ||
	    (robot->blobfinder->blobs[blob_i].range/1000 < 0.17) );
    
    playerc_position2d_set_cmd_vel( robot->position2d,
				    0,
				    0,
				    0,
				    1);
    return EXIT_SUCCESS;
}

int
roboboca_robot_focus_on_nearest_blob( roboboca_robot_t * robot )
{
    int i = 0;
    
    unsigned int blob_area = -1;
    int blob_i = -1;
    
    do
    {
	playerc_client_read( robot->client );
	blob_area = 0;
	blob_i = 0;

	for( i = 0 ; i < robot->blobfinder->blobs_count ; i++ )
	{
	    if( robot->blobfinder->blobs[i].color == BLOB_COLOR )
	    {
		if( robot->blobfinder->blobs[i].area > blob_area )
		{
		    blob_area = robot->blobfinder->blobs[i].area;
		    blob_i = i;
		}
	    }
	}

	if( robot->blobfinder->blobs[blob_i].x > 40 )
	    playerc_position2d_set_cmd_vel( robot->position2d,
					    0,
					    0,
					    -0.2,
					    1);
	else if(robot->blobfinder->blobs[blob_i].x < 40 )
	    playerc_position2d_set_cmd_vel( robot->position2d,
					    0,
					    0,
					    0.2,
					    1);
    }while( (robot->blobfinder->blobs[blob_i].x > 41) ||
	    (robot->blobfinder->blobs[blob_i].x < 39) );
    
    playerc_position2d_set_cmd_vel( robot->position2d,
				    0,
				    0,
				    0,
				    1);
    if( robot->blobfinder->blobs[blob_i].range/1000 > 0.6 )
	return EXIT_FAILURE;
    
    return EXIT_SUCCESS;
}

void
roboboca_robot_point_to_blob( roboboca_robot_t * robot,
			      const double blob_x,
			      const double blob_y )
{
    double angle = 0;
    double vangularfinal = 0;
    do
    {
	playerc_client_read( robot->client );
	
	angle = atan2( (blob_y - roboboca_robot_get_global_py(robot)),
		       (blob_x - roboboca_robot_get_global_px(robot)) );
	angle = roboboca_minor_distance_rad( angle, robot->position2d->pa );	
	vangularfinal = angle;
	
        /* Multiplica o valor de rotação para que ele seja
	 * significativo */
	vangularfinal *= 3;

	/* Não deixa que a rotação seja muito alta */
	if( vangularfinal > 2 )
	    vangularfinal = 2;
	else if( vangularfinal < -2 )
	    vangularfinal = -2;
	playerc_position2d_set_cmd_vel( robot->position2d,
					0,
					0,
					vangularfinal,
					1);
    }while(fabs(angle) > 0.1);
    playerc_position2d_set_cmd_vel( robot->position2d,
				    0,
				    0,
				    0,
				    1);
}

/******************************************************************************/
